#include "Calib.hpp"
#include "Transform.hpp"
#include <assert.h>

HandInEye::HandInEye(Mat_<double> target2Cam, Mat_<double> gripper2base, Mat &cam2gripper, cv::HandEyeCalibrationMethod method)
{
    this->__Transform(target2Cam, gripper2base);
    this->__calculate(method);
    this->_H_cam2gripper.copyTo(cam2gripper);
}

void HandInEye::__Transform(Mat_<double> target2Cam, Mat_<double> gripper2base)
{
    assert(target2Cam.size() == gripper2base.size());

    Mat H, R, T;
    for (int i = 0; i < target2Cam.rows; i++)
    {
        // 目标相对于相机位姿 转换成R和T
        H = CarteToHMat(target2Cam.row(i), false, "");
        this->_H_target2cam.push_back(H);
        HSplitToRT(H, R, T);
        this->_R_target2cam.push_back(R);
        this->_T_target2cam.push_back(T);

        // 末端相对于极坐标系位姿 转换成R和T
        H = CarteToHMat(gripper2base.row(i), false, "xyz");
        this->_H_gripper2base.push_back(H);
        HSplitToRT(H, R, T);
        this->_R_gripper2base.push_back(R);
        this->_T_gripper2base.push_back(T);
    }
}

void HandInEye::__calculate(cv::HandEyeCalibrationMethod method)
{
    //TSAI计算速度最快
    calibrateHandEye(_R_gripper2base, _T_gripper2base, _R_target2cam, _T_target2cam, _R_cam2gripper, _T_cam2gripper, method);
    this->_H_cam2gripper = RTMergeToH(_R_cam2gripper, _T_cam2gripper);
}